#include <cdefBF537.h>
#include "robochallenge.h"
#include "print.h"

#define DMA_BUF1     0x01000000  // address in SDRAM for DMA transfer of frames from camera
#define DMA_BUF2     0x01300000  // second DMA buffer for double buffering
#define FRAME_BUF    0x01600000  // address in SDRAM for staging images for processing/jpeg
#define FRAME_BUF2   0x01900000  // second buffer for storing reference frame
#define FRAME_BUF3   0x01C00000  // another buffer for edge data

#define MAX_BLOBS  63
#define MAX_COLORS 17  // reserve color #16 for internal use
#define MIN_BLOB_SIZE 50
#define index(xx, yy)  ((yy * imgWidth + xx) * 2) & 0xFFFFFFFC

extern unsigned int imgWidth, imgHeight;
unsigned int blobx1[MAX_BLOBS], blobx2[MAX_BLOBS], bloby1[MAX_BLOBS], bloby2[MAX_BLOBS], blobcnt[MAX_BLOBS];

unsigned int ymax[MAX_COLORS], ymin[MAX_COLORS], umax[MAX_COLORS], umin[MAX_COLORS], vmax[MAX_COLORS], vmin[MAX_COLORS];
unsigned int ykleur[4], ukleur[4], vkleur[4];
unsigned int blobx1[MAX_BLOBS], blobx2[MAX_BLOBS], bloby1[MAX_BLOBS], bloby2[MAX_BLOBS], blobcnt[MAX_BLOBS];
unsigned int hist0[256], hist1[256], mean[3];
char filter;

void init_colors() 
{
	ykleur[0] = 0;
	ukleur[0] = 0;
	vkleur[0] = 0;
	ykleur[1] = 0;
	ukleur[1] = 255;
	vkleur[1] = 255;
}

void filterControl(char value)
{
	filter = value;
}

int color_filter(unsigned char *input, int width, int height)
{
	int i;
	for(i = 0;i<38400;i++)
	{
		if(input[i*4] < 140 && input[i*4] > 100 && input[i*4+2] < 100 ) //groen
		{
			input[i*4] = 0;
			input[(i*4)+1] = 0;
			input[(i*4)+2] = 0;
			input[(i*4)+3] = 0;
		}
		/*else if(input[i*4+1] < 110 && input[i*4] > 146 && input[i*4+2] < 110) //blauw
		{
			input[i*4] = 255;
			input[(i*4)+1] = 0;
			input[(i*4)+2] = 0;
			input[(i*4)+3] = 0;
		}*/
		else if(input[i*4+1] > 200 && input[i*4] < 90 && input[i*4] > 50 && input[i*4+2] > 120 && input[i*4+2] < 140) //geel
		{
			input[i*4] = 0;
			input[(i*4)+1] = 255;
			input[(i*4)+2] = 255;
			input[(i*4)+3] = 255;
		}
		/*else if(input[i*4+1] > 146 && input[i*4] < 110 && input[i*4+2] > 146) //paars
		{
			input[i*4] = 255;
			input[(i*4)+1] = 0;
			input[(i*4)+2] = 255;
			input[(i*4)+3] = 0;
		}*/
		else
		{
			input[i*4] = 128;
			input[(i*4)+1] = 16;
			input[(i*4)+2] = 128;
			input[(i*4)+3] = 16;
		}
	}
	return 1;
}

void controlMotor(char cMotorNumber, long lPosition)
{
	printf("%2.2dM%3.3d",cMotorNumber,lPosition);
}

void start_robochallenge()
{
	printf("Lets start the automatic robot control\n\r");
	init_colors();
	filter = 1;
	for(;;)
	{
		findBlobs(); // zoek groen
		//delayMS(500);
	}
}

char findBlobs()
{
	unsigned int ix, iy;
    grab_frame(1);
    ix = vblob((unsigned char *)FRAME_BUF, (unsigned char *)FRAME_BUF3, 0);
    for (ix=0; ix<MAX_BLOBS; ix++)
		if (blobcnt[ix] < MIN_BLOB_SIZE)
			break;
	int temp = -1;
    for (iy=0; iy<ix; iy++) 
	{
		if(blobcnt[iy] > blobcnt[temp])
		{
			temp = iy;
		}
		//printf(" %d - %d %d %d %d\n\r", blobcnt[iy], blobx1[iy], blobx2[iy], bloby1[iy], bloby2[iy], blobcnt[iy]);
    }

	if(blobcnt[temp] > 1000)
	{
		printf("m99#");
		printf("%d - %d %d %d %d\n\r", blobcnt[iy], blobx1[iy], blobx2[iy], bloby1[iy], bloby2[iy], blobcnt[iy]);
	}
	else
	{
		int temp123 = blobx2[temp] - blobx1[temp];
		temp123 = temp123 / 2;
		int kaas = blobx1[temp] + temp123;
		if(kaas > 200)
		{
			printf("m10#");
		}
		else if(kaas < 120)
		{
			printf("m01#");
		}
		else
		{
			printf("m11#");
		}
	}
	return iy;
}

void grab_frame (char impFilter) {
    move_image((unsigned char *)DMA_BUF1, (unsigned char *)DMA_BUF2, 
            (unsigned char *)FRAME_BUF, imgWidth, imgHeight);  
	if(filter && impFilter)
	{
		color_filter((unsigned char *)FRAME_BUF, imgWidth, imgHeight); //code in robochallenge.c
	}
}

void move_image(unsigned char *src1, unsigned char *src2, unsigned char *dst, unsigned int width, unsigned int height) 
{
    unsigned char *src;
    unsigned short *isrc, *idst;
    unsigned int ix;
        
    if (*pDMA0_CURR_ADDR < (void *)src2)
        src = src2;
    else
        src = src1;
    
    isrc = (unsigned short *)src;
    idst = (unsigned short *)dst;
    for (ix = 0; ix < (width * height); ix++)
        *idst++ = *isrc++;
    return;
}

// return number of blobs found that match the search color
// algorithm derived from "Using a Particle Filter for Gesture Recognition", Alexander Gruenstein
//    http://www.mit.edu/~alexgru/vision/
unsigned int vblob(unsigned char *frame_buf, unsigned char *blob_buf, unsigned int ii) {
    unsigned int ix, iy, xx, yy, y, u, v, tmp;
    unsigned char curBlob, vL, vTL, vT, vTR, vME;


    for (curBlob=0; curBlob<MAX_BLOBS; curBlob++) {
        blobcnt[curBlob] = 0;
        blobx1[curBlob] = imgWidth;
        blobx2[curBlob] = 0;
        bloby1[curBlob] = imgHeight;
        bloby2[curBlob] = 0;
    }
        
    // tag all pixels in blob_buf[]    
    //     matching = 1  
    //     no color match = 0
    // thus all matching pixels will belong to blob #1
    for (yy=0; yy<imgHeight; yy++) {
        for (xx=0; xx<imgWidth; xx+=2) {   
            ix = index(xx,yy);
            iy = xx + (yy * imgWidth);
            y = (unsigned int)frame_buf[ix];
            u = (unsigned int)frame_buf[ix+1];
            v = (unsigned int)frame_buf[ix+2];

            if ((y == ykleur[ii]) && (u == ukleur[ii]) && (v == vkleur[ii]))
			{
                blob_buf[iy] = 1;
			}
            else
			{
                blob_buf[iy] = 0;
			}
        }
    }


    curBlob = 1;
    for (yy=1; yy<imgHeight; yy++) {
        for (xx=2; xx<(imgWidth-2); xx+=2) {
            ix = xx + (yy * imgWidth);
            vL = 0; vTL = 0; vT = 0; vTR = 0;
            vME = 0;
            if (blob_buf[ix] == 1) {
                vL =  blob_buf[ix-2];    // left
                vTL = blob_buf[(ix - imgWidth) - 2];   // top left
                vT =  blob_buf[ix - imgWidth];  // top
                vTR = blob_buf[(ix - imgWidth) + 2];   // top right
                
                if (vL)
                    vME = vL;
                if (vTL)
                    vME = vTL;  // guaranteed same as vL by previous iteration
                if (vT) {
                    if ((vL != 0) && (vL != vT))      // we have a U connection
                        blob_merge(blob_buf, vT, vL); // change all vT's to vL's
                    else
                        vME = vT;
                }
                if (vTR) {
                    if ((vTL != 0) && (vTL != vTR))
                        blob_merge(blob_buf, vTR, vTL); // change all vTR's to vTL's
                    else
                        vME = vTR;
                }
                if (vME == 0) {
                    vME = curBlob;
                    curBlob++;
                    if (curBlob >= MAX_BLOBS)  // max blob limit exceeded
                        return 0;
                }
                blob_buf[ix] = vME;
            }
        }
    }




    // measure the blobs
    for (yy=0; yy<imgHeight; yy++) {
        for (xx=0; xx<imgWidth; xx+=2) {
            ix = xx + (yy * imgWidth);
            iy = blob_buf[ix];
            if (iy) {
                blobcnt[iy]++;
                if (xx < blobx1[iy])
                    blobx1[iy] = xx;
                if (xx > blobx2[iy])
                    blobx2[iy] = xx;
                if (yy < bloby1[iy])
                    bloby1[iy] = yy;
                if (yy > bloby2[iy])
                    bloby2[iy] = yy;
            }
        }
    }


    // compress the blob array
    for (xx=0; xx<=curBlob; xx++)
        if (blobcnt[xx] < MIN_BLOB_SIZE)
            blobcnt[xx] = 0;
            
    for (xx=0; xx<curBlob; xx++) {
        if (blobcnt[xx] == 0) {
            for (yy=xx+1; yy<=curBlob; yy++) {
                if (blobcnt[yy]) {
                    blobcnt[xx] = blobcnt[yy];
                    blobx1[xx] = blobx1[yy];
                    blobx2[xx] = blobx2[yy];
                    bloby1[xx] = bloby1[yy];
                    bloby2[xx] = bloby2[yy];
                    blobcnt[yy] = 0;
                    break;
                }
            }
        }
    }
    
    iy = 0;
    for (xx=0; xx<=curBlob; xx++) {
        if (blobcnt[xx])
            iy++;
        else
            break;
    }
    curBlob = iy;


    // sort blobs by size, largest to smallest pixel count
    for (xx=0; xx<=curBlob; xx++) {
        if (blobcnt[xx] == 0)  // no more blobs
            break;
        for (yy=xx+1; yy<=curBlob; yy++) {
            if (blobcnt[yy] == 0)
                break;
            if (blobcnt[xx] < blobcnt[yy]) {
                tmp = blobcnt[xx];
                blobcnt[xx] = blobcnt[yy];
                blobcnt[yy] = tmp;
                tmp = blobx1[xx];
                blobx1[xx] = blobx1[yy];
                blobx1[yy] = tmp;
                tmp = blobx2[xx];
                blobx2[xx] = blobx2[yy];
                blobx2[yy] = tmp;
                tmp = bloby1[xx];
                bloby1[xx] = bloby1[yy];
                bloby1[yy] = tmp;
                tmp = bloby2[xx];
                bloby2[xx] = bloby2[yy];
                bloby2[yy] = tmp;
            }
        }
    }
    return curBlob;
}

// merge blobs, changing any pixel in blob_buf[] in old_blob to new_blob
void blob_merge(unsigned char *blob_buf, unsigned char old_blob, unsigned char new_blob) {
    int ix;
    for (ix=0; ix<(imgWidth*imgHeight>>1); ix++)
        if (blob_buf[ix] == old_blob)
            blob_buf[ix] = new_blob;
}

void getColor()
{
	char ch1 = getch() & 0x0F;
	char ch2 = getch() & 0x0F;
	char ch3 = getch() & 0x0F;
	char ch4 = getch() & 0x0F;
	int i1 = ch1*1000 + ch2*100 + ch3*10 + ch4;
	ch1 = getch() & 0x0F;
	ch2 = getch() & 0x0F;
	ch3 = getch() & 0x0F;
	ch4 = getch() & 0x0F;
	int i2 = ch1*1000 + ch2*100 + ch3*10 + ch4;
	grab_frame(0);
	int ix = vpix((unsigned char *)FRAME_BUF, i1, i2);
	printf("##v %d %d %d\n\r",
		((ix>>16) & 0x000000FF),  // Y1
		((ix>>24) & 0x000000FF),  // U
		((ix>>8) & 0x000000FF));   // V
}

unsigned int vpix(unsigned char *frame_buf, unsigned int xx, unsigned int yy) {
        unsigned int ix;
        ix = index(xx,yy); 
        return    ((unsigned int)frame_buf[ix] << 24) +    // returns UYVY packed into 32-bit word
                        ((unsigned int)frame_buf[ix+1] << 16) +
                        ((unsigned int)frame_buf[ix+2] << 8) +
                        (unsigned int)frame_buf[ix+3];
}